#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

#ifndef PI
#define PI 3.141592654
#endif

int main(int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr laser;
  pcl::PointCloud<pcl::PointXYZ>::Ptr real_world;
  pcl::PointCloud<pcl::PointXYZ> final;
  Eigen::Matrix4f best_matrix;

  laser.reset(new pcl::PointCloud<pcl::PointXYZ>);
  real_world.reset(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::io::loadPCDFile("../wing_pcl/scan_back_80.pcd", *real_world);
  pcl::io::loadPCDFile("../wing_pcl/scan_back.pcd", *laser);

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setMaximumIterations(1000);
  icp.setTransformationEpsilon(0.000001);
  icp.setEuclideanFitnessEpsilon(0.00001);
  icp.setInputTarget(laser);
  icp.setInputSource(real_world);
  icp.align(final);

  pcl::io::savePCDFile("final.pcd", final);

  std::cout << "has coveraged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
  best_matrix = icp.getFinalTransformation();
  std::cout << best_matrix << std::endl;

  double xx = best_matrix(0,3);
  double yy = best_matrix(1,3);
  double tth = acos(best_matrix(0,0));

  printf("Transform: [%.3f, %.3f, %.2f]\n", xx, yy, tth/PI*180);

  return 0;
}